/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package batailledesardennes.util.geomFloat.path;

import batailledesardennes.util.FastMath;
import batailledesardennes.util.geomFloat.GeomUtils;
import batailledesardennes.util.geomFloat.Point;
import batailledesardennes.util.geomFloat.Polygon;
import java.util.ArrayList;
import java.util.HashSet;
import java.util.List;
import java.util.Set;

/**
 *
 * @author cghislai
 */
public class NodeHandler {

    private final List<Node> nodes;
    private final List<Node> closeNodes;
    private final List<Obstacle> obstacles;
    private float maxConnectionDistance;

    public NodeHandler() {
        nodes = new ArrayList<Node>();
        closeNodes = new ArrayList<Node>();
        obstacles = new ArrayList<Obstacle>();
    }

    public void addObstacle(Obstacle obstacle) {
        if (obstacles.contains(obstacle)) {
            return;
        }
        Polygon polygon = obstacle.getPolygon();
        fillCloseNode(obstacle.getPolygon().getCenter());
        // check if obsacle obstructs existing connections
        for (Node checkNode : closeNodes) {
            final Point checkNodePt = checkNode.getPoint();
            Set<Node> toRemoveNodes = new HashSet<Node>();
            for (Node connectedNode : checkNode.getConnectedNodes()) {
                final Point connectedNodePt = connectedNode.getPoint();
                if (!polygon.mayIntersectWithLine(checkNodePt, connectedNodePt)) {
                    continue;
                }
                if (!polygon.intersectsWithLineSegment(checkNodePt, connectedNodePt)) {
                    continue;
                }
                toRemoveNodes.add(connectedNode);
            }
            for (Node toRemoveNode : toRemoveNodes) {
                toRemoveNode.getConnectedNodes().remove(checkNode);
                checkNode.getConnectedNodes().remove(toRemoveNode);
            }
        }
        obstacles.add(obstacle);
        for (Node node : obstacle.getNodes()) {
            addNode(node);
        }
    }

    public void removeObstacle(Obstacle obstacle) {
        if (!obstacles.contains(obstacle)) {
            return;
        }
        obstacles.remove(obstacle);
        nodes.removeAll(obstacle.getNodes());
        fillCloseNode(obstacle.getPolygon().getCenter());
        // Reconnect all nodes around connected to obstacle
        List<Node> toReconnectNodes = new ArrayList<Node>();
        for (Node obstacleNode : obstacle.getNodes()) {
            toReconnectNodes.addAll(obstacleNode.getConnectedNodes());
            obstacleNode.clearConnectedNodes();
        }
        for (Node node : toReconnectNodes) {
            connectNode(node);
        }
    }

    public void addNode(Node node) {
        if (nodes.contains(node)) {
            return;
        }
        nodes.add(node);
        connectNode(node);
    }

    public void removeNode(Node node) {
        nodes.remove(node);
        for (Node connectedNode : node.getConnectedNodes()) {
            connectNode(connectedNode);
        }
    }

    private void connectNode(Node node) {
        node.clearConnectedNodes();
        for (Node n : getConnectableNodes(node)) {
            if (isConnectionUseful(node, n)) {
                connectNodeNoCheck(node, n);
            }
            if (isConnectionUseful(n, node)) {
                connectNodeNoCheck(n, node);
            }
        }
        // connect to prev and next in polygon
        if (node.getObstacle() != null) {
            List<Node> polNodes = node.getObstacle().getNodes();
            int i = polNodes.indexOf(node);
            int nextI = (i >= polNodes.size() - 1 ? 0 : i + 1);
            int prevI = (i == 0 ? polNodes.size() - 1 : i - 1);
            Node nextNode = polNodes.get(nextI);
            Node prevNode = polNodes.get(prevI);
            connectNodeNoCheck(node, nextNode);
            connectNodeNoCheck(node, prevNode);
        }
    }

    private void connectNodeNoCheck(Node node1, Node node2) {
        if (!node1.getConnectedNodes().contains(node2)) {
            node1.getConnectedNodes().add(node2);
        }
    }

    public List<Node> getConnectableNodes(Node node) {
        fillCloseNode(node.getPoint());
        float maxDistSq = maxConnectionDistance * maxConnectionDistance;
        List<Node> connectableNodes = new ArrayList<Node>();
        for (Node closeNode : closeNodes) {
            if (closeNode == node) {
                continue;
            }
            float distSq = node.getPoint().distanceSq(closeNode.getPoint());
            if (distSq > maxDistSq) {
                continue;
            }
            if (isObstacleInTheWay(node.getPoint(), closeNode.getPoint())) {
                continue;
            }
            connectableNodes.add(closeNode);
        }
        return connectableNodes;
    }

    private boolean isConnectionUseful(Node node1, Node node2) {
        Point p1 = node1.getPoint();
        Point p2 = node2.getPoint();
        if (node1.getObstacle() != null) {
            Obstacle obs1 = node1.getObstacle();
            // prev and next nodes on the obstacle
            int idx1 = obs1.getNodes().indexOf(node1);
            Node prevNode1 = obs1.getNodes().get(idx1 == 0 ? obs1.getNodes().size() - 1 : idx1 - 1);
            Node nextNode1 = obs1.getNodes().get(idx1 + 1 == obs1.getNodes().size() ? 0 : idx1 + 1);

            // Check if next or previous point are in the way
            int checkPrev1 = GeomUtils.ccw(p1, p2, prevNode1.getPoint());
            int checkNext1 = GeomUtils.ccw(p1, p2, nextNode1.getPoint());
            if (checkPrev1 == 0 || checkNext1 == 0) {
                return false;
            }
        }
        if (node2.getObstacle() != null) {
            Obstacle obs2 = node2.getObstacle();
            int idx2 = obs2.getNodes().indexOf(node2);
            Node prevNode2 = obs2.getNodes().get(idx2 == 0 ? obs2.getNodes().size() - 1 : idx2 - 1);
            Node nextNode2 = obs2.getNodes().get(idx2 + 1 == obs2.getNodes().size() ? 0 : idx2 + 1);
            int checkPrev2 = GeomUtils.ccw(p1, p2, prevNode2.getPoint());
            int checkNext2 = GeomUtils.ccw(p1, p2, nextNode2.getPoint());
            if (checkPrev2 == 0 || checkNext2 == 0) {
                return false;
            }

            int prevToNodeCcw1 = GeomUtils.ccw(p1, prevNode2.getPoint(), p2);
            int nodeToNextCcw1 = GeomUtils.ccw(p1, p2, nextNode2.getPoint());
            if (prevToNodeCcw1 == nodeToNextCcw1) {
                return false;
            }
        }
        return true;
    }

    private boolean isObstacleInTheWay(Point p1, Point p2) {
        // Check if any obstacle is in the way
        // shorten first the segment so that source and target node
        // dont intersect with their obstacle
        Point src = p1.getCopy();
        Point dst = p2.getCopy();
        float angle = src.findAngle(dst);
        float dist = 0.001f;
        src.translate(dist * FastMath.fastCos(angle), dist * FastMath.fastSin(angle));
        dst.translate(-dist * FastMath.fastCos(angle), -dist * FastMath.fastSin(angle));
        for (Obstacle o : obstacles) {
            if (!o.getPolygon().mayIntersectWithLine(src, dst)) {
                continue;
            }
            if (o.getPolygon().intersectsWithLineSegment(src, dst)) {
                return true;
            }
        }
        return false;
    }

    private void fillCloseNode(Point point) {
       closeNodes.clear();
        for (Node node : nodes) {
           Point p = node.getPoint();
           float distSq = p.distanceSq(point);
           if (distSq < maxConnectionDistance*maxConnectionDistance) {
              closeNodes.add(node);
           }
        }
    }

    public float getMaxConnectionDistance() {
        return maxConnectionDistance;
    }

    public void setMaxConnectionDistance(float maxConnectionDistance) {
        this.maxConnectionDistance = maxConnectionDistance;
    }

    public List<Node> getNodes() {
        return nodes;
    }

    public List<Obstacle> getObstacles() {
        return obstacles;
    }
}
